#include <ServoSoft.h>
#include <MsTimer2.h>
#include <defArduinoParaArduino.h>

#define TEMPO 500

// Ma - Frente - Direita
#define Ma_b1 7 // fio branco
#define Ma_b2 8 // fio laranja

#define ENABLE 6

// Mb - Frente - Esquerda
#define Mb_b1 5 // marrom escuro
#define Mb_b2 4 // marrom claro

// Mc - Tras - Direita
#define Mc_b1 14  // verde claro
#define Mc_b2 15 // verde escuro

// Md - Tras - Esquerda
#define Md_b1 13 // fio azul claro
#define Md_b2 16 //fio azul escuro

 
char msg[16];
char msgSize;
char response[16];


 Servo servo01; // frontal - direita
 Servo servo02; // frontal - esquerda
 Servo servo03; // traseira - esquerda
 Servo servo04; // traseira - direita
 
 Servo servo; // para testes
 
 
void refreshServos()
{
  Servo::refresh();
}


int serialGetMsg()
{
  int i;  
  i=0;
  
  
    while (Serial.available())
  //    while(msg[i]!='-')
        msg[i++] = Serial.read();
     
    
  if(i>0)
  {
    msg[i]='\0';
//    Serial.println(msg);
  }
  
  msgSize=i;
  return i;
}


void avante()
{
    servo01.write(90);
    servo02.write(0);
    servo03.write(90);
    servo04.write(0);
    Servo::refresh();

    delay(700);    
}

void aoLado()
{
    servo01.write(0);
    servo02.write(90);
    servo03.write(0);
    servo04.write(90);
    Servo::refresh();    

    delay(700);
}

void girar()
{
    servo01.write(45);
    servo02.write(45);
    servo03.write(45);
    servo04.write(45);
    Servo::refresh();    
    
    delay(500);
}

void stop()
{
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,LOW);
  
  delay(500);
}

void go_fwd(long int time)
{
  avante();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}

void go_rev(long int time)
{
  avante();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  
  delay(time);
  stop();
}

void go_right(long int time)
{
  aoLado();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  delay(time);
  stop();
}

void go_left(long int time)
{
  aoLado();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}

void turn_ccw(long int time)
{
  girar();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  delay(time);
  stop();
}

void turn_cw(long int time)
{
  girar();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}

void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  Serial.begin(115200);
  
  pinMode(ENABLE,OUTPUT);
  
  pinMode(Ma_b1,OUTPUT);
  pinMode(Ma_b2,OUTPUT);
  
  pinMode(Mb_b1,OUTPUT);
  pinMode(Mb_b2,OUTPUT);
  
  pinMode(Mc_b1,OUTPUT);
  pinMode(Mc_b2,OUTPUT);
  
  pinMode(Md_b1,OUTPUT);
  pinMode(Md_b2,OUTPUT);
  
  
  digitalWrite(ENABLE,HIGH);
  

/* --- Configuracao para os servos MG946R */

  servo01.attach(9); 
  servo01.setMinimumPulse(840);
  servo01.setMaximumPulse(2680);

  servo02.attach(10); 
  servo02.setMinimumPulse(800);
  servo02.setMaximumPulse(2680);

  servo03.attach(11); 
  servo03.setMinimumPulse(900);
  servo03.setMaximumPulse(2830);
  
  servo04.attach(12); 
  servo04.setMinimumPulse(880);
  servo04.setMaximumPulse(2830);
  
  
  MsTimer2::start();
} 
 

void demonstracao()
{
    go_fwd(1500);
    go_right(1500);
    go_rev(1500);
    go_left(750);
    go_fwd(750);
    turn_ccw(500);
    go_fwd(500);
    turn_cw(1000);
    go_fwd(500);
    go_left(500);
    go_rev(500);
    go_right(00);
    turn_ccw(2000);
}

void pegaInstrucao()
{
long int arg = 0;  

arg = (msg[1] - '0')*100;
arg = arg + (msg[2] - '0')*10;
arg = arg + (msg[3] - '0')*1;

arg = arg*100000; // 100 ==>> 1000ms

      switch( msg[4] )
      {
        case OMNI_FWD: go_fwd(arg);
                       break;
        
        case OMNI_DIR: go_right(arg);
                      break;
                  
        case OMNI_ESQ: go_left(arg);
                      break;
                  
        case OMNI_REV: go_rev(arg);
                      break;
                  
        case OMNI_CCW: turn_ccw(arg);
                       break;
                  
       case OMNI_CW: turn_cw(arg);
                     break;

       default : break;           
      
      }

}

void loop() 
{  
    
    avante();
    
    delay(5);// wait for serial input
    
  int n = serialGetMsg();
  
  if(n)
  {
    if( (msg[0]=='<') && (msg[5]=='>') )
    {
      Serial.println("ACK");  
        
      pegaInstrucao( );
               
    }
    else
      Serial.println("NACK");
  }
         
      
  delay(30); 
    
} 
